#include "vag.h"

CanTxMsg mVagEngStream =
{
    0x740, 0x18db33f1,
    CAN_ID_STD, CAN_RTR_DATA,
    5,
    0x00, 0x00, 0x02, 0x21, 0x00
};


CanTxMsg mVagBcmStream =
{
    0x74A, 0x18db33f1,
    CAN_ID_STD, CAN_RTR_DATA,
    8,
    0x03, 0x22, 0x00, 0x00, 0x55, 0x55, 0x55, 0x55
};

CanTxMsg mVagIpcStream =
{
    0x714, 0x18db33f1,
    CAN_ID_STD, CAN_RTR_DATA,
    8,
    0x03, 0x22, 0x22, 0x00, 0x55, 0x55, 0x55, 0x55
};


VagEngPidMap mVagEngPidMap[ENG_DATA_SIZE] =
{
    { ENG_DATA_RPM,           0x01, 0x03, 2, SUPPORTED },
    { ENG_DATA_VS,            0x05, 0x09, 2, SUPPORTED },
    { ENG_DATA_ECT,           0x01, 0x06, 2, SUPPORTED },
    { ENG_DATA_IAT,           0x04, 0x0c, 2, SUPPORTED },
    { ENG_DATA_APP,           0x00, 3, 1, UNSUPPORTED },
    { ENG_DATA_TP,            0x03, 0x09, 2, SUPPORTED },
    { ENG_DATA_ERT,           0x00, 3, 1, UNSUPPORTED },
    { ENG_DATA_LOAD,          0x02, 0x06, 2, SUPPORTED },
    { ENG_DATA_BATTVOLT,      0x04, 0x06, 2, SUPPORTED },
    { ENG_DATA_REALFUELCO,    0x04, 0x06, 2, UNSUPPORTED },
    { ENG_DATA_FUELLEVEL,     0x01, 0x24, 1, UNSUPPORTED },
    { ENG_DATA_ODO,           0x11, 0x04, 2, UNSUPPORTED },
    { ENG_DATA_LTFT,          0x74, 0x09, 2, SUPPORTED },
    { ENG_DATA_STFT,          0x74, 0x06, 2, SUPPORTED },
    { ENG_DATA_MISFIRE1,      0x0f, 0x03, 2, SUPPORTED },
    { ENG_DATA_MISFIRE2,      0x0f, 0x06, 2, SUPPORTED },
    { ENG_DATA_MISFIRE3,      0x0f, 0x09, 2, SUPPORTED },
    { ENG_DATA_MISFIRE4,      0x10, 0x03, 2, SUPPORTED },
    { ENG_DATA_MISFIRE5,      0x00, 3, 1, UNSUPPORTED },
    { ENG_DATA_MISFIRE6,      0x00, 3, 1, UNSUPPORTED },
    { ENG_DATA_FCLS,          0x00, 3, 1, UNSUPPORTED },
    { ENG_DATA_KEYSTATUS,     0x00, 3, 1, UNSUPPORTED },
    { ENG_DATA_HO2S1,         0x00, 3, 1, UNSUPPORTED },
    { ENG_DATA_HO2S2,         0x00, 3, 1, UNSUPPORTED },
    { ENG_DATA_MAP,           0x00, 3, 1, UNSUPPORTED },
    { ENG_DATA_INJECTPULSE,   0x00, 3, 1, UNSUPPORTED },
    { ENG_DATA_OILPRESSURE,   0x00, 3, 1, UNSUPPORTED },
    { ENG_DATA_OILLEVELSTATUS,0x00, 3, 1, UNSUPPORTED },
};

VagBcmPidMap mVagBcmPidMap[BCM_DATA_SIZE] =
{
    {BCM_DATA_LFWIN,          {0x18, 0x08}, 0x04, 0x01, SUPPORTED},
    {BCM_DATA_RFWIN,          {0x19, 0x08}, 0x04, 0x01, SUPPORTED},
    {BCM_DATA_LRWIN,          {0x18, 0X08}, 0X05, 0X01, SUPPORTED},
    {BCM_DATA_RRWIN,          {0x19, 0x08}, 0x05, 0x01, SUPPORTED},
    {BCM_DATA_DDA,            {0x19, 0x08}, 0x05, 0x01, UNSUPPORTED},
    {BCM_DATA_PDA,            {0x19, 0x08}, 0x05, 0x01, UNSUPPORTED},
    {BCM_DATA_RRDA,           {0x19, 0x08}, 0x05, 0x01, UNSUPPORTED},
    {BCM_DATA_LRDA,           {0x19, 0x08}, 0x05, 0x01, UNSUPPORTED},
    {BCM_DATA_SUNROOF,        {0x19, 0x08}, 0x05, 0x01, UNSUPPORTED},
    {BCM_DATA_PARKLAMP,       {0x19, 0x08}, 0x05, 0x01, UNSUPPORTED},
    {BCM_DATA_HEADLAMP,       {0x19, 0x08}, 0x05, 0x01, UNSUPPORTED},
    {BCM_DATA_CHARGESTATUS,   {0x19, 0x08}, 0x05, 0x01, UNSUPPORTED},
    {BCM_DATA_BATTCURRENT,    {0x19, 0x08}, 0x05, 0x01, UNSUPPORTED},
    {BCM_DATA_BATTSTATUS,     {0x19, 0x08}, 0x05, 0x01, UNSUPPORTED},
    {BCM_DATA_BATTVOLT,       {0x19, 0x08}, 0x05, 0x01, UNSUPPORTED},
    {BCM_DATA_ODO,            {0x19, 0x08}, 0x05, 0x01, UNSUPPORTED},
};

VagEngPidMap mVagIpcPidMap[IPC_DATA_SIZE] =
{
    { IPC_DATA_REALFUELCO,    0x98, 0x04, 2, SUPPORTED },
    { IPC_DATA_ODO,           0x03, 0x04, 2, SUPPORTED },
    { IPC_DATA_FUELLEVEL,     0x06, 0x04, 1, SUPPORTED },
};

VehicleProcessOps mVagProcessOps;
OS_TMR vag_keepalive_timer;
uint8_t mVagEngineOn = TRUE;

void vag_init(Vehicle *vehicle)
{
    OS_ERR err;

    logi("%s", __func__);
    mVagProcessOps.check_engine_on = vag_check_engine_on;
    mVagProcessOps.process_vin_code = vag_process_vin_code;
    mVagProcessOps.process_eng_data = vag_process_eng_data;
    mVagProcessOps.process_bcm_data = vag_process_bcm_data;
    mVagProcessOps.process_ipc_data = vag_process_ipc_data;

    vehicle->processOps = &mVagProcessOps;

    //create timer
    OSTmrCreate((OS_TMR *)&vag_keepalive_timer,
            (CPU_CHAR   *)"VAG KEEPALIVE TIMER",
            (OS_TICK     )OS_CFG_TMR_TASK_RATE_HZ * 2,
            (OS_TICK     )0,
            (OS_OPT      )OS_OPT_TMR_PERIODIC,
            (OS_TMR_CALLBACK_PTR)vag_keepalive_timer_callback,
            (void       *)0,
            (OS_ERR     *)&err);
    bsp_can_filter(0x300, 0x300, 0x3ff, 0x3ff);
}

void vag_start_timer(uint16_t timeout)
{
    OS_ERR err;

    OSTmrSet((OS_TMR *)&vag_keepalive_timer,
            (OS_TICK     )OS_CFG_TMR_TASK_RATE_HZ * timeout,
            (OS_TICK     )0,
            (OS_TMR_CALLBACK_PTR)vag_keepalive_timer_callback,
            (void       *)0,
            (OS_ERR     *)&err);
    OSTmrStart((OS_TMR *)&vag_keepalive_timer, &err);
    if(err != OS_ERR_NONE) {
        loge("start timer failed, err = %d", err);
    }
}

void vag_stop_timer(void)
{
    OS_ERR err;

    OSTmrStop((OS_TMR *)&vag_keepalive_timer, OS_OPT_TMR_NONE, (void *)0, &err);
    if(err != OS_ERR_NONE) {
        loge("stop timer failed, err = %d", err);
    }
}

void vag_keepalive_timer_callback(void *p_arg)
{
    logi("%s: callback timeout", __func__);
    vag_keepalive();
}

void vag_initialize(void)
{
    CanTxMsg initialize[3] =
    {
        {
            0x200, 0x00,
            CAN_ID_STD, CAN_RTR_DATA,
            7,
            0x01, 0xc0, 0x00, 0x10, 0x00, 0x03, 0x01
        },
        {
            0x740, 0x00,
            CAN_ID_STD, CAN_RTR_DATA,
            6,
            0xa0, 0x0f, 0x8a, 0xff, 0x32, 0xff
        },
        {
            0x740, 0x00,
            CAN_ID_STD, CAN_RTR_DATA,
            6,
            0xa0, 0x0f, 0x8a, 0xff, 0x4a, 0xff
        },
    };
    uint8_t i;

    for(i = 0; i < 3; i++) {
        bsp_can_send(&initialize[i]);
        os_delay_ms(100);
    }
}

//vag data functions
uint8_t vag_check_engine_on(void)
{
    logi("%s", __func__);
    return mVagEngineOn;
}

void vag_process_vin_code(VinCode *code)
{
    uint8_t i, j, k;
    uint8_t ret;
    CanRxMsg rxMsg;
    uint8_t offset;
    uint8_t size;
    uint8_t pid;
    uint8_t first;
    uint8_t second;
    uint16_t index;
    uint16_t buf_index;
    uint8_t temp_buf[128];
    uint8_t package_count = 0;
    uint8_t bytes = 0;
    uint8_t sub_off;
    uint8_t index_of_pid;
    CanTxMsg vinPackage =
    {
        0x7df, 0x18db33f1,
        CAN_ID_STD, CAN_RTR_DATA,
        8,
        0x02, 0x09, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00
    };
    CanTxMsg continuePackage =
    {
        0x7df, 0x18db33f1,
        CAN_ID_STD, CAN_RTR_DATA,
        8,
        0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
    };

    logi("%s", __func__);
    bsp_can_clear();
    bsp_can_filter(0x7e8, 0x7e8, 0x7ff, 0x7ff);
    //check the pid is supported
    size = 17;
    offset = 5;
    //send data stream
    buf_index = 0;
    memset(temp_buf, 0x00, 128);
    ret = vehicle_can_session(&vinPackage, &rxMsg);
    if(ret) {
        first = rxMsg.Data[0];
        second = rxMsg.Data[1];
        index = index_of_buf(rxMsg.Data, 8, pid);
        if(first == 0x10) {
            //long package
            bytes = rxMsg.Data[1];
            package_count = (bytes - 6) / 7;
            if((bytes - 6) % 7 > 0) {
                package_count ++;
            }
            for(j = index + 1; j < 8; j++) {
                temp_buf[buf_index ++] = rxMsg.Data[j];
            }
            //send continue package
            bsp_can_send(&continuePackage);
            for(j = 0; j < package_count; j++) {
                ret = vehicle_can_recv(&rxMsg);
                if(ret) {
                    for(k = 1; k < 8; k++) {
                        temp_buf[buf_index ++] = rxMsg.Data[k];
                    }
                } else {
                    break;
                }
            }
        } else if(first != 0x10 && second == 0x61){
            //short package
            for(j = index + 1; j < 8; j++) {
                temp_buf[buf_index ++] = rxMsg.Data[j];
            }
        } else {
            //error
            return;
        }

        code->updated = TRUE;
        memcpy(code->code, temp_buf + offset, size);
        logi("vin: ");
        for(i = 0; i < 17; i++) {
            printf("%02x ", code->code[i]);
        }
        printf("\r\n");
    } else {
        return;
    }
    os_delay_ms(100);
}

void vag_process_eng_data(EngineDataMap *dataMap)
{
    uint8_t i, j, k;
    uint8_t support;
    uint8_t pid;
    uint8_t offset;
    uint8_t size;
    CanRxMsg rxMsg;
    uint8_t data[100];
    uint8_t rx_index;
    uint8_t bytes;
    uint8_t package_count;
    uint8_t index = 0xb0;
    CanTxMsg endPackage =
    {
        0x740, 0x18db33f1,
        CAN_ID_STD, CAN_RTR_DATA,
        1,
        0xb0
    };
    CanTxMsg stopPackage =
    {
        0x740, 0x18db33f1,
        CAN_ID_STD, CAN_RTR_DATA,
        1,
        0xa8
    };
    uint8_t end_index = 0x00;

    vag_start_timer(2);
    vag_initialize();
    bsp_can_clear();
    bsp_can_filter(0x300, 0x300, 0x3ff, 0x3ff);
    for(i = 0; i < ENG_DATA_SIZE; i++) {
        os_delay_ms(200);
        vehicle_lock();
        memset(data, 0x00, 100);
        support = mVagEngPidMap[i].support;
        //check wether this id is supported?
        if(support == UNSUPPORTED) {
            vehicle_unlock();
            continue;
        }
        pid = mVagEngPidMap[i].pid;
        offset = mVagEngPidMap[i].offset;
        size = mVagEngPidMap[i].size;
        mVagEngStream.Data[0] = 0x10 | (i & 0xf);
        mVagEngStream.Data[4] = pid;
        //send stream msg
        if(vehicle_can_session(&mVagEngStream, &rxMsg)) {
            //response B1
            if(rxMsg.DLC == 1) {
                //rx_index = rxMsg.Data[0];
            } else {
                vehicle_unlock();
                continue;
            }
            index = 0;
            //receive first package
            if(vehicle_can_recv(&rxMsg)) {
                bytes = rxMsg.Data[2];
                package_count = 1 + (bytes - 5) / 7;
                if((bytes - 5) % 7 > 0) {
                    package_count ++;
                }
                for(j = 3; j < rxMsg.DLC; j++) {
                    data[index ++] = rxMsg.Data[j];
                }
                for(j = 1; j < package_count; j++) {
                    if(vehicle_can_recv(&rxMsg)) {
                        if(rxMsg.Data[0] < 0x10) {
                            j--;
                            continue;
                        }
                        for(k = 1; k < rxMsg.DLC; k++) {
                            data[index ++] = rxMsg.Data[k];
                        }
                    }
                }
                //send end package
                end_index = 0xb0 | ((end_index + package_count) & 0xf);
                endPackage.Data[0] = end_index;
                bsp_can_send(&endPackage);
            }

            dataMap->data[i].id = i;
            dataMap->data[i].updated = TRUE;
            dataMap->data[i].len = size;
            memcpy(dataMap->data[i].data, data + offset, size);
            if(i == ENG_DATA_RPM) {
                mVagEngineOn = TRUE;
            }
        } else {
            if(i == ENG_DATA_RPM) {
                mVagEngineOn = FALSE;
            }
        }
    }
    vehicle_unlock();
    vag_stop_timer();
    //bsp_can_send(&stopPackage);
}

void vag_process_bcm_data(BcmDataMap *dataMap)
{
    uint8_t i;
    uint8_t support;
    uint8_t cmd1, cmd2;
    uint8_t offset;
    uint8_t size;
    CanRxMsg rxMsg;

    bsp_can_clear();
    bsp_can_filter(0x7b4, 0x7b4, 0x7ff, 0x7ff);
    for(i = 0; i < BCM_DATA_SIZE; i++) {
        os_delay_ms(100);
        vehicle_lock();
        support = mVagBcmPidMap[i].support;
        //check wether this id is supported?
        if(support == UNSUPPORTED) {
            vehicle_unlock();
            continue;
        }
        cmd1 = mVagBcmPidMap[i].cmd[0];
        cmd2 = mVagBcmPidMap[i].cmd[1];
        offset = mVagBcmPidMap[i].offset;
        size = mVagBcmPidMap[i].size;
        mVagBcmStream.Data[2] = cmd1;
        mVagBcmStream.Data[3] = cmd2;
        //send stream msg
        if(vehicle_can_session(&mVagBcmStream, &rxMsg)) {
            dataMap->data[i].id = i;
            dataMap->data[i].updated = TRUE;
            dataMap->data[i].len = size;
            memcpy(dataMap->data[i].data, rxMsg.Data + offset, size);
        }
        vehicle_unlock();
    }
}

void vag_process_ipc_data(IpcDataMap *dataMap)
{
    uint8_t i;
    uint8_t support;
    uint8_t pid;
    uint8_t offset;
    uint8_t size;
    CanRxMsg rxMsg;

    bsp_can_clear();
    bsp_can_filter(0x77e, 0x77e, 0x7ff, 0x7ff);
    for(i = 0; i < IPC_DATA_SIZE; i++) {
        os_delay_ms(100);
        vehicle_lock();
        support = mVagIpcPidMap[i].support;
        //check wether this id is supported?
        if(support == UNSUPPORTED) {
            vehicle_unlock();
            continue;
        }
        pid = mVagIpcPidMap[i].pid;
        offset = mVagIpcPidMap[i].offset;
        size = mVagIpcPidMap[i].size;
        mVagIpcStream.Data[3] = pid;
        //send stream msg
        if(vehicle_can_session(&mVagIpcStream, &rxMsg)) {
            dataMap->data[i].id = i;
            dataMap->data[i].updated = TRUE;
            dataMap->data[i].len = size;
            memcpy(dataMap->data[i].data, rxMsg.Data + offset, size);
        }
        vehicle_unlock();
    }
}

void vag_keepalive(void)
{
    CanRxMsg rxMsg;
    CanTxMsg keepalivePackage =
    {
        0x740, 0x18db33f1,
        CAN_ID_STD, CAN_RTR_DATA,
        1,
        0xa3
    };

    bsp_can_clear();
    bsp_can_filter(0x300, 0x300, 0x3ff, 0x3ff);

    if(!vehicle_can_session(&keepalivePackage, &rxMsg)) {
        loge("%s: error", __func__);
    }
}
